#include "ServoController.h"
#include "DSP2833x_Device.h"
#include "DataFile.h"

extern SDataFile df;

bool ServoController::Init()
{
	// enable "EN" sig. on L6203 
	df.Servo5Duty = 100;
 	df.Servo6Duty = 0; // kill outputs
 	df.Servo7Duty = 0; // kill outputs
 	
 	// default values
 	df.TailPotKp = 1.0f; // 5% dutycycle per degree error (fix deadzone somehow?!)
 	df.TailPotVoltsZero = 1.75; // mid value!
	df.TailPotVolts = 1.75; // default (mid value)

	df.TailPotIntegr = 0; // reset
	df.TailPotKi = 0.05f; // I gain
	
	return true;
}

// this is UPDATE method -> must be called often!
void ServoController::SetPos(float angle)
{
	// get pot. position
	float potAngle = (df.TailPotVolts - df.TailPotVoltsZero)/1.65f * 150; // TODO: calibrate this! 150 deg for full range?
	df.TailPotAngle = potAngle; // update datafile
	
	// calculate error
	float error = angle - potAngle;

	df.TailPotIntegr += error*df.TailPotKi;
	// Limit I
	if( df.TailPotIntegr > 20 ) df.TailPotIntegr = 20;
	if( df.TailPotIntegr < -20 ) df.TailPotIntegr = -20;
	
	// run controller!
	float u = df.TailPotKp * error + df.TailPotIntegr;
	
	// limit u
	if( u > 50 ) u = 50;
	if( u < -50 ) u = -50;
	
	if( u > 0 ) u += 8;
	if( u < 0 ) u -= 8;
	
	// run motor
	if( u > 0 )
	{
		// forward
		df.Servo6Duty = u;
		df.Servo7Duty = 0;
	}
	else
	{
		// backward
		df.Servo6Duty = 0;
		df.Servo7Duty = -u;
	}
}
